
#include "socket_can_client_raw.h"
#include "can_client.h"
#include <thread>
#include "logging.h"
#include <iostream>
#include "common_util.h"
#include "safeuqeue.h"
#include "can_client.h"

// extern std::shared_ptr<SafeQueue::SafeQueue<canbus::CanFrame>> sQueue;
extern std::shared_ptr<SafeQueue::SafeQueue<canbus::CanFrame>> sCanRecvQueue;
extern std::shared_ptr<SafeQueue::SafeQueue<canbus::CanFrame>> sCanSendQueue;

#define CAN_ID_MASK 0x1FFFF800U // can_filter mask

namespace canbus
{
  // namespace can
  // {
  bool SocketCanClientRaw::Init(std::string port_)
  {
    port_name = port_;
    LOG_INFO("=======SocketCanClientRaw ==== Init!");
    return true;
  }

  SocketCanClientRaw::~SocketCanClientRaw()
  {
    if (sock_can_fd)
    {
      Stop();
    }
  }

  ErrorCode SocketCanClientRaw::Start()
  {
    int ret;
    if (is_started_)
    {
      return ErrorCode::OK;
    }

    sock_can_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    LOG_INFO("Start sock_can_fd :{}", sock_can_fd);
    if (sock_can_fd < 0)
    {
      LOG_ERROR("   open device error code {}", sock_can_fd);
      return ErrorCode::CAN_CLIENT_ERROR_BASE;
    }

    const std::string can_name = port_name;
    std::strcpy(ifr.ifr_name, can_name.c_str());
    if (ioctl(sock_can_fd, SIOCGIFINDEX, &ifr) < 0)
    {
      LOG_ERROR("ioctl error");
      return ErrorCode::CAN_CLIENT_ERROR_BASE;
    }

    // bind socket to network interface

    addr.can_family = AF_CAN;
    // addr.can_ifindex = ifr.ifr_ifindex;
    addr.can_ifindex = 0;
    ret = bind(sock_can_fd, reinterpret_cast<struct sockaddr *>(&addr), sizeof(addr));

    if (ret < 0)
    {
      LOG_ERROR("bind socket to network interface error code: {}", ret);
      return ErrorCode::CAN_CLIENT_ERROR_BASE;
    }

    is_started_ = true;
    return ErrorCode::OK;
  }

  void SocketCanClientRaw::Stop()
  {
    if (is_started_)
    {
      is_started_ = false;

      int ret = close(sock_can_fd);
      if (ret < 0)
      {
        LOG_ERROR("close error code:: {},{}", ret, GetErrorString(ret));
      }
    }
  }

  // Synchronous transmission of CAN messages

  // ErrorCode SocketCanClientRaw::Send(const std::vector<canbus::CanFrame> &frames,
  //                                    int32_t *const frame_num)
  ErrorCode SocketCanClientRaw::Send(canbus::CanFrame &frames,
                                     int32_t *const frame_num)

  {
    static struct sockaddr_can addr;
    static struct ifreq ifr;
    static struct can_frame c_frame;
    if (!is_started_)
    {
      std::cout << " can client has not been initiated! Please init first!";
      return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
    }

    // 以下是对CAN接口进行初始化，如设置CAN接口名，即当我们用ifconfig命令时显示的名字
    strcpy((char *)(ifr.ifr_name), port_name.c_str());
    ioctl(sock_can_fd, SIOCGIFINDEX, &ifr);
    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;
    // 将刚生成的套接字与CAN套接字地址进行绑定
    bind(sock_can_fd, (struct sockaddr *)&addr, sizeof(addr));

    c_frame.can_id = CAN_EFF_FLAG | frames.id;
    c_frame.can_dlc = 8;
    memset(c_frame.data, 0, 8);
    memcpy(c_frame.data, frames.data, 8);
    // printf("Send frame.can_id  %x:\r\n",c_frame.can_id);
    auto ret = sendto(sock_can_fd, &c_frame, sizeof(struct can_frame), 0, (struct sockaddr *)&addr, sizeof(addr));
    if (ret != sizeof(struct can_frame))
    {
      return ErrorCode::CAN_CLIENT_ERROR_BASE;
    }
    // }
    return ErrorCode::OK;
  }

  // buf size must be 8 bytes, every time, we receive only one frame
  ErrorCode SocketCanClientRaw::Receive(std::vector<CanFrame> *const frames, int32_t *const frame_num)
  {

    can_frame recv_frame_;
    CanFrame cf;
    auto ret = read(sock_can_fd, &recv_frame_, sizeof(recv_frame_));
    if (ret < 0)
    {
      std::cout << "receive message failed, error code: " << ret;
      return ErrorCode::CAN_CLIENT_ERROR_BASE;
    }
    if ((recv_frame_.can_id >> 31) == 1) // 判断是否为扩展帧
    {
      recv_frame_.can_id &= CAN_EFF_MASK;
    }
    cf.id = recv_frame_.can_id;
    cf.len = recv_frame_.can_dlc;
    std::memcpy(cf.data, recv_frame_.data, recv_frame_.can_dlc);
    frames->push_back(cf);

    return ErrorCode::OK;
  }

  std::string SocketCanClientRaw::GetErrorString(const int32_t /*status*/)
  {
    return "";
  }

  void SocketCanClientRaw::RecvThreadFunc()
  {
    int32_t receive_error_count = 0;
    int32_t receive_none_count = 0;
    const int32_t ERROR_COUNT_MAX = 10;
    auto default_period = 1;

    while (1)
    {
      std::vector<CanFrame> can_buf;
      // int32_t frame_num = MAX_CAN_RECV_FRAME_LEN;
      int32_t frame_num = 1;
      if (Receive(&can_buf, &frame_num) != ErrorCode::OK)
      {
        std::cout << "Received " << receive_error_count << " error messages.";
        std::this_thread::sleep_for(std::chrono::milliseconds(default_period));
        // continue;
      }
      receive_error_count = 0;

      if (can_buf.size() != static_cast<size_t>(frame_num))
      {
        std::cout << "Receiver buf size [" << can_buf.size()
                  << "] does not match can_client returned length["
                  << frame_num << "].";
      }
      sCanRecvQueue->push(can_buf[0]);
      std::this_thread::sleep_for(std::chrono::milliseconds(default_period));

      receive_none_count = 0;
    }
  }

  void SocketCanClientRaw::SendThreadFunc()
  {
    sched_param sch;
    sch.sched_priority = 99;
    pthread_setschedparam(pthread_self(), SCHED_FIFO, &sch);

    int64_t sleep_interval = 5;

    int frame_num = 1;
    canbus::CanFrame send_can_frame;
    canbus::CanFrame can_frame_;
    std::vector<canbus::CanFrame> send_frames;

    while (1)
    {
      if (!sCanSendQueue->empty())
      {
        sCanSendQueue->tryPop(can_frame_);
        send_can_frame.id = can_frame_.id;
        memcpy(send_can_frame.data, can_frame_.data, 8);
        // send_frames.push_back(send_can_frame); // 有没有必要再加一个队列
        // frame_num = send_frames.size();
        // if (Send(send_frames, &frame_num) != ErrorCode::OK) /// send
        // console_show_can_msg(send_can_frame);
        if (Send(send_can_frame, &frame_num) != ErrorCode::OK) /// send
        {
          std::cout << "Send msg failed:"; //<< can_frames.data;
        }
      }

      std::this_thread::sleep_for(std::chrono::microseconds(sleep_interval));
    }
    std::cout << "Can client sender thread stopped!";
  }

  // } // namespace can
} // namespace canbus
